﻿using System;
using System.Collections.Generic;
using System.Text;
using Cambia.CoreLib;
using LocalizationTest.Tools;
using System.Drawing;

namespace LocalizationTest
{
  class BearingSensorAmbiguous : Sensor
  {
    public MapWithLandmarks.LandMarkClasses landmarkClass;
    private MapWithLandmarks.LandmarkIDs landmarkID;
    double headingDirection;
    double angleOfView;

    public BearingSensorAmbiguous()
    {
      this.landmarkClass = MapWithLandmarks.LandMarkClasses.corner;
      this.headingDirection = 0;
      this.angleOfView = Math.PI / 2.0;
    }

    public BearingSensorAmbiguous(MapWithLandmarks.LandMarkClasses landmarkClass)
    {
      this.landmarkClass = landmarkClass;
      this.headingDirection = 0;
      this.angleOfView = Math.PI / 3.0;
    }

    public BearingSensorAmbiguous(MapWithLandmarks.LandMarkClasses landmarkClass, double headingDirection, double angleOfView)
    {
      this.landmarkClass = landmarkClass;
      this.headingDirection = headingDirection;
      this.angleOfView = angleOfView;
    }

    internal override void SimulateMeasurement(WorldState worldState, Map map, ActionAndObservationHistory actionAndObservationHistory)
    {
      for (int i = 0; i < (map as MapWithLandmarks).XPositionOfLandmark.Length; i++)
      {
        landmarkID = (MapWithLandmarks.LandmarkIDs)i;
        if (MapWithLandmarks.GetLandmarkClass(landmarkID) == landmarkClass)
        {
          Measurement performedMeasurement = PredictForSimulation(worldState, map);
          actionAndObservationHistory.Add(performedMeasurement);
        }
      }
    }

    public override double GetMeasurementDeviation(WorldState worldState, Map map, Measurement measurement)
    {
      double minDifference = double.MaxValue;
      double averageDifference = 0;
      int averageCounter = 0;
      for (int i = 0; i < (map as MapWithLandmarks).XPositionOfLandmark.Length; i++)
      {
        landmarkID = (MapWithLandmarks.LandmarkIDs)i;
        if (MapWithLandmarks.GetLandmarkClass(landmarkID) == landmarkClass)
        {
          Measurement prediction = PredictPerfect2(worldState, map, false);
          if (prediction != null)
          {
            double difference = prediction.GetDifference(measurement);
            if (Math.Abs(difference) < Math.Abs(minDifference)) minDifference = difference;
            averageDifference += difference;
            averageCounter++;
          }
        }
      }
      averageDifference /= averageCounter;
//      DebugTextTool.Output("avg counter: " + averageCounter);
      return minDifference;
      //return averageDifference;
    }

    public override Measurement PredictForSimulation(WorldState worldState, Map map)
    {
      bool limitView = DebugKeyTool.IsActive("_measurement:limited view");
      double sigma = Globals.bearingMeasurementSigma;
      double gaussianError = RandomProvider.NextNormal() * sigma; // Math.Sqrt(sigma);
      double systematicError = 0.5;

      switch (Globals.selfLocatorProperties.SensorNoise)
      {
        case NoiseType.equal: break; //todo
        case NoiseType.noNoise: gaussianError = 0; systematicError = 0; break;
        case NoiseType.gaussianAndSystematic: break;
        case NoiseType.gaussianNoSystematic: systematicError = 0; break;
        case NoiseType.noGaussianButSystematic: gaussianError = 0; break;
      }
      AngleMeasurement measurement = (AngleMeasurement)PredictPerfect2(worldState, map, limitView);
      if (measurement == null)
        return null;
      else
        return new AngleMeasurement(measurement.Angle + gaussianError + systematicError, this);
    }

    public override Measurement PredictPerfect(WorldState worldState, Map map)
    {
      return PredictPerfect2(worldState, map, false);
    }
    private Measurement PredictPerfect2(WorldState worldState, Map map, bool limitView)
    {
      double x_robot = 0;
      double y_robot = 0;
      double theta_robot = 0;
      double x_landmark = 0;
      double y_landmark = 0;

      if (worldState is WorldState1D)
      {
        x_robot = (worldState as WorldState1D).Position;
      }

      if (worldState is WorldState2D)
      {
        x_robot = (worldState as WorldState2D).Vector.x;
        y_robot = (worldState as WorldState2D).Vector.y;
      }

      if (worldState is WorldStatePose2D)
      {
        x_robot = (worldState as WorldStatePose2D).Pose.translation.x;
        y_robot = (worldState as WorldStatePose2D).Pose.translation.y;
        theta_robot = (worldState as WorldStatePose2D).Pose.rotation;
      }

      if (map is MapWithLandmarks)
      {
        x_landmark = (map as MapWithLandmarks).XPositionOfLandmark[(int)landmarkID];
        y_landmark = (map as MapWithLandmarks).YPositionOfLandmark[(int)landmarkID];
      }
      if (map is MapWithSingleLandmark)
      {
        x_landmark = (map as MapWithSingleLandmark).XPositionOfLandmark[(int)landmarkID];
        y_landmark = (map as MapWithSingleLandmark).YPositionOfLandmark[(int)landmarkID];
      }

      double distance = Math.Sqrt(Math.Pow(y_landmark - y_robot, 2) + Math.Pow(x_landmark - x_robot, 2));
      double angle = Math.Atan2(y_landmark - y_robot, x_landmark - x_robot) - theta_robot;
      if (
        Math.Abs(MathTools.Normalize(angle - headingDirection)) < angleOfView / 2.0 &&
        distance < 100 || 
        !limitView )
        return new AngleMeasurement(angle, this);
      else
        return null;
    }

    public override double GetMeasurementProbability(WorldState worldState, Map map, Measurement measurement)
    {
      double expectedMeasurement = (PredictPerfect(worldState, map) as AngleMeasurement).Angle;
      double conductedMeasurement = (measurement as AngleMeasurement).Angle;
      return Globals.Gaussian(conductedMeasurement, Globals.bearingMeasurementSigma * 20, expectedMeasurement);
    }

    public override void Draw(Measurement measurement, WorldState worldState, double shade, string drawingID)
    {
      double startX = 0;
      double startY = 0;
      double endX = 0;
      double endY = 0;
      double rotationOffset = 0;

      if (worldState is WorldState1D)
      {
        startX = (worldState as WorldState1D).Position;
        startY = 0;
      }
      if (worldState is WorldState2D)
      {
        startX = (worldState as WorldState2D).Vector.x;
        startY = (worldState as WorldState2D).Vector.y;
      }
      if (worldState is WorldStatePose2D)
      {
        startX = (worldState as WorldStatePose2D).Pose.translation.x;
        startY = (worldState as WorldStatePose2D).Pose.translation.y;
        rotationOffset = (worldState as WorldStatePose2D).Pose.rotation;
      }
      endX = startX + Math.Cos((measurement as AngleMeasurement).Angle + rotationOffset) * 150.0;
      endY = startY + Math.Sin((measurement as AngleMeasurement).Angle + rotationOffset) * 150.0;

      Color color = Color.FromArgb((int)(255.0), (int)(200.0 * shade), (int)(200.0 * shade));
      color = MapWithLandmarks.GetColorOfLandmark((measurement.sensor as BearingSensorAmbiguous).landmarkID);
      color = DrawingTools.GetShadedColor(shade, ref color);
      double width = 5;
      if (shade == 0.9999) {color = Color.Gray; width = 1;}
      DebugDrawingTool.CreateLine(drawingID, new Vector2(startX, startY), new Vector2(endX, endY), width, color);
    }
    public override string ToString()
    {
      return this.GetType().Name + ";" + landmarkClass.ToString();
    }
  }
}
